#include "HandModel.h"
#include <math.h>
#include <stdint.h>

// Dinh nghia 1 class Hmfinger
HmFinger::HmFinger()
{

}

HmFinger::~HmFinger()
{

}

void HmFinger::setFingerType(FingerType typef)
{
	type=typef;
}

void HmFinger::setDefaultParameter()
{
	setFingerParameter(0,	9.5f,	9.0f,	40.0f);
	setFingerParameter(1,	8.5f,	8.0f,	28.0f);
	setFingerParameter(2,	7.5f,	7.0f,	20.0f);

	setFingerRotation(0,	Eigen::Vector3f(0,0,0));
	setFingerRotation(1,	Eigen::Vector3f(0,0,0));
	setFingerRotation(2,	Eigen::Vector3f(0,0,0));
}

void HmFinger::setFingerParameter(int bone_index,float radious1,float radious2,float length)
{
	bone[bone_index].length = length;
	bone[bone_index].radius1 = radious1;
	bone[bone_index].radius2 = radious2;
}

void HmFinger::setFingerRotation(int bone_index,Eigen::Vector3f t_rotation)
{
	bone[bone_index].rotation = t_rotation;
}

HandModel::HandModel(void)
{
	lastTime = glfwGetTime();
	frame = 0;
	
	handScale = PIXEL_PER_MM;
	wirst_rotation=Eigen::Vector3f(0,0,0);
	wirst_position=Eigen::Vector3f(0,0,-1000);

	finger_1.setDefaultParameter();
	finger_2.setDefaultParameter();
	finger_2.bone[0].length = 45;
	finger_2.bone[1].length = 30;
	finger_3.setDefaultParameter();
	
	finger_thumb.setFingerParameter(0,	13,	11,	40);
	finger_thumb.setFingerParameter(1,	11,	10,	35);
	finger_thumb.setFingerParameter(2,	10,	9,	25);
	finger_thumb.setFingerRotation(0,	Eigen::Vector3f(0,0,60));
	finger_thumb.setFingerRotation(1,	Eigen::Vector3f(0,0,0));
	finger_thumb.setFingerType(HM_FINGER_THUMB);

	finger_little.setFingerParameter(0,	7.5f,	7.3f,	33);
	finger_little.setFingerParameter(1,	7.3f,	7.0f,	20);
	finger_little.setFingerParameter(2,	7.0f,	6.8f,	20);
	finger_little.setFingerRotation(0,	Eigen::Vector3f(0,0,-2));
	finger_little.setFingerType(HM_FINGER_LITTLE);
}

HandModel::~HandModel(void)
{
}

void HandModel::getHypothesis(Hypothesis h)
{
	wirst_position = Eigen::Vector3f(h.wirst_Position[0],h.wirst_Position[1],-h.wirst_Position[2]);
	wirst_rotation = Eigen::Vector3f(h.wirst_Rotation[0],h.wirst_Rotation[1],h.wirst_Rotation[2]);

	//rotation[1]-[2]-[3] follow x-axis
	//rotation[0] follow z-axis
	finger_thumb.setFingerRotation(0,Eigen::Vector3f(h.thumb_Rotation[1],0,h.thumb_Rotation[0]));
	finger_thumb.setFingerRotation(1,Eigen::Vector3f(h.thumb_Rotation[2],0,0));
	finger_thumb.setFingerRotation(2,Eigen::Vector3f(h.thumb_Rotation[3],0,0));

	finger_1.setFingerRotation(0,Eigen::Vector3f(h.finger1_Rotation[1],0,h.finger1_Rotation[0]));
	finger_1.setFingerRotation(1,Eigen::Vector3f(h.finger1_Rotation[2],0,0));
	finger_1.setFingerRotation(2,Eigen::Vector3f(h.finger1_Rotation[3],0,0));

	finger_2.setFingerRotation(0,Eigen::Vector3f(h.finger2_Rotation[1],0,h.finger2_Rotation[0]));
	finger_2.setFingerRotation(1,Eigen::Vector3f(h.finger2_Rotation[2],0,0));
	finger_2.setFingerRotation(2,Eigen::Vector3f(h.finger2_Rotation[3],0,0));

	finger_3.setFingerRotation(0,Eigen::Vector3f(h.finger3_Rotation[1],0,h.finger3_Rotation[0]));
	finger_3.setFingerRotation(1,Eigen::Vector3f(h.finger3_Rotation[2],0,0));
	finger_3.setFingerRotation(2,Eigen::Vector3f(h.finger3_Rotation[3],0,0));

	finger_little.setFingerRotation(0,Eigen::Vector3f(h.little_Rotation[1],0,h.little_Rotation[0]));
	finger_little.setFingerRotation(1,Eigen::Vector3f(h.little_Rotation[2],0,0));
	finger_little.setFingerRotation(2,Eigen::Vector3f(h.little_Rotation[3],0,0));
}

// Ve dot ngon tay voi 1 bac tu do
void HandModel::drawHandBone(handBone hm_handbone)
{
	//glRotatef(hm_handbone.rotation[2],0,0,1);
	glRotatef(hm_handbone.rotation[0],1,0,0); //rotate x : Ve bac tu do thu nhat (Goc xoay theo phuong X)
	drawSolidConeRotated(hm_handbone.radius1,hm_handbone.radius2,hm_handbone.length,25);
	glTranslatef(0,hm_handbone.length,0); // Tinh tien theo chieu Y 1 doan = do dai dot ngon tay
	drawSolidSphere(hm_handbone.radius2,15,15); // Ve khop cau
}

// Ve dot ngon tay voi 2 bac tu do 
void HandModel::drawHandBone(handBone hm_handbone,bool MultipleDOF,bool thumb)
{
	// Neu khong phai ngon cai
	if (MultipleDOF && !thumb)
	{
		glRotatef(hm_handbone.rotation[2],0,0,1);//rotate z: Ve dot ngon tay voi goc xoay theo phuong z
		drawHandBone(hm_handbone);
		return;
	}

	if (thumb)
	{
		glRotatef(hm_handbone.rotation[0],0,1,0);//rotate y 
		glRotatef(hm_handbone.rotation[2],0,0,1);//rotate z
		
		glTranslatef(0,hm_handbone.length/2,0);  // Vi la elipsoid nen dich tinh tien ve tam elipse
		drawSolidElipse(hm_handbone.radius1,
			hm_handbone.length/2 + hm_handbone.radius2,
			hm_handbone.radius1,25,25);          // Ve elipse
		glTranslatef(0,hm_handbone.length/2,0);
		drawSolidSphere(hm_handbone.radius2,15,15);  // Ve khop cau
	}
}

// Ve ca ngon tay
void HandModel::drawFinger(HmFinger hm_finger){

	// Ve dot thu nhat
	if (hm_finger.type == HM_FINGER_THUMB) {
		drawHandBone(hm_finger.bone[0],true,true);
	}else{  // Neu khong phai ngon cai thi co them khop cau
		drawSolidSphere(hm_finger.bone[0].radius1,15,15);
		drawHandBone(hm_finger.bone[0],true,false);
	}
	glPushMatrix(); // Dich goc ve toi vi tri vua dat con tro ve
		if (hm_finger.type == HM_FINGER_THUMB)	glRotatef(45,0,1,0); // Ve dot tiep theo
		drawHandBone(hm_finger.bone[1]);
		glPushMatrix();  // Lai dich goc ve toi vi tri dot 2
			drawHandBone(hm_finger.bone[2]);  // Ve dot cuoi cung
		glPopMatrix();  // Quay lai goc ve dot 2
	glPopMatrix(); // Quay lai goc ve dot 1
}


// Ve long ban tay
void HandModel::drawPalm(float width,float height,float depth)
{
	float musculeSize = finger_thumb.bone[0].radius2;
	glTranslatef(width/10,depth/2,0);  // dich toi tam long ban tay

	glPushMatrix();  // luu lai vi tri tam ve
	glTranslatef(0,0,musculeSize - depth/2);
	drawSolidElipse(width/2,musculeSize*1.5,musculeSize,25,25);  // Ve elipse tren
	glPopMatrix();

	glPushMatrix();
	glRotatef(-90,1,0,0);
	drawSolidElipseCylinder(width/2,depth/2,height,15);  // Ve hinh tru (mau vang)
	glPopMatrix(); 

	glPushMatrix();
	glTranslatef(0,height,musculeSize - depth/2);
	drawSolidElipse(width/2,musculeSize*1.5f,musculeSize,25,25);  // Ve elipsoid duoi
	glPopMatrix();
}

void HandModel::drawHand()
{
	glTranslatef(wirst_position[0],wirst_position[1],wirst_position[2]);

	glPushMatrix();
	glRotatef(wirst_rotation[0], 1.f, 0.f, 0.f);  
	glRotatef(wirst_rotation[1], 0.f, 1.f, 0.f);
	glRotatef(wirst_rotation[2], 0.f, 0.f, 1.f);
	glScalef(handScale,handScale,handScale);// scale from mm to pixel
	drawPalmAndFiveFingers();// draw hand data
	glPopMatrix();
}

// Ve ca ban tay
void HandModel::drawPalmAndFiveFingers()
{
	// Vi tri con tro o co tay
	//thumb
	glPushMatrix();  // Luu lai vi tri con tro
	glTranslatef(-20,10,0);  // Dich toi vi tri dot 1 ngon cai
	drawFinger(finger_thumb);  // Ve ngon cai
	glPopMatrix();

	//other 4 fingers
	float root1[3] = {	-20.f,		75.f,		0.f} ;
	float root2[3] = {	0.f,		75.f,		0.f} ;
	float root3[3] = {	20.f,		75.f,		0.f} ;
	float root4[3] = {	38.f,		70.f,		0.f} ;//little finger

	glPushMatrix();
	drawPalm(80,60,20);
	glPopMatrix();

	glPushMatrix();
	glTranslatef(root1[0],root1[1],root1[2]);
	drawFinger(finger_1);
	glPopMatrix();

	glPushMatrix();
	glTranslatef(root2[0],root2[1],root2[2]);
	drawFinger(finger_2);
	glPopMatrix();

	glPushMatrix();
	glTranslatef(root3[0],root3[1],root3[2]);
	drawFinger(finger_3);
	glPopMatrix();

	glPushMatrix();
	glTranslatef(root4[0],root4[1],root4[2]);
	drawFinger(finger_little);
	glPopMatrix();

	//// Measure speed
	//double currentTime = glfwGetTime();
	//frame++;

	//if ( currentTime - lastTime >= 1.0 ){ // If last prinf() was more than 1 sec ago
	//	// printf and reset timer
	//	printf("%f ms/frame\n", 1000.0/double(frame));
	//	lastTime = currentTime;
	//}
}


cv::Mat HandModel::get_gl_depth(int width,int height)
{
	if (buffer)
	{
		delete[] buffer;
	}
	buffer = new GLfloat[width*height];

	glReadBuffer(GL_FRONT); // Lay buffer phia truoc
	glReadPixels(0, 0, width, height, GL_DEPTH_COMPONENT, GL_FLOAT,buffer); // Lay do sau
	//printf("capture = %f \n",buffer);
	cv::Mat depthMap = cv::Mat(height,width,CV_32FC1,buffer);  // Dua vao opencv chuyen sang da muc xam

	return depthMap;
}

void HandModel::get_gl_depth_at(double x,double y)
{
	float data = 0;
	glReadBuffer(GL_FRONT);
	glReadPixels(int(x), 480-int(y), 1, 1, GL_DEPTH_COMPONENT, GL_FLOAT,&data);
	printf("data(%d,%d) = %f \n",int(x),int(y),data);
}